# numpy_client.py
import numpy as np
import time
import threading
import socketio
import logging
import queue


from lerobot.common.robot_devices.control_configs import ControlPipelineConfig
from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
import draccus
import torch


cli_args = ['--robot.type=so101', '--robot.cameras={}', '--control.type=teleoperate']

cfg = draccus.parse(config_class=ControlPipelineConfig, config_path=None, args=cli_args)
robot = ManipulatorRobot(cfg.robot)


if not robot.is_connected:
    robot.connect()


# 启用详细日志
logging.basicConfig(level=logging.INFO)

sio = socketio.Client(logger=True, engineio_logger=True)

SERVER_URL = "http://127.0.0.1:5000"
SERVER_URL = "http://47.109.17.68:8868"
SEND_RATE = 1000  # 每秒发送30次

# 创建发送队列和锁
send_queue = queue.Queue(maxsize=50)
send_lock = threading.Lock()
last_sent_time = time.time()

def generate_data():
    """生成数据并放入队列"""
    while True:
        # 生成随机numpy数组 (fp32, 长度6)
        leader_pos = {}
        for name in robot.leader_arms:
            leader_pos[name] = robot.leader_arms[name].read("Present_Position")
        data = leader_pos['main']
        # 转换为列表以便JSON序列化
        data_list = data.tolist()
        
        # 如果队列满了，丢弃最旧的数据
        if send_queue.full():
            try:
                send_queue.get_nowait()
            except queue.Empty:
                pass
        
        # 将数据放入队列
        send_queue.put(data_list)
        # time.sleep(1/SEND_RATE)

def send_data():
    """从队列发送数据"""
    global last_sent_time
    
    while True:
        if sio.connected and not send_queue.empty():
            try:
                # 获取队列中的数据
                data = send_queue.get()
                
                # 使用锁确保发送间隔
                with send_lock:
                    current_time = time.time()
                    elapsed = current_time - last_sent_time
                    min_interval = 1/SEND_RATE
                    
                    # 确保最小发送间隔
                    # if elapsed < min_interval:
                    #     time.sleep(min_interval - elapsed)
                    
                    # 发送数据
                    sio.emit('numpy_data', data, namespace='/numpy')
                    last_sent_time = time.time()
                    
                    # print(f"Sent numpy data: {data}")  # 减少日志输出
            except Exception as e:
                print(f"Error sending data: {e}")
        else:
            time.sleep(0.1)  # 短暂休眠

@sio.event(namespace='/numpy')
def connect():
    print('Connected to server on /numpy namespace')

@sio.event(namespace='/numpy')
def disconnect():
    print('Disconnected from /numpy namespace')

if __name__ == '__main__':
    try:
        # 连接到服务端
        sio.connect(SERVER_URL, namespaces=['/numpy'])
        print(f"Connected to {SERVER_URL} on namespaces: /numpy")
        
        # 启动数据生成线程
        gen_thread = threading.Thread(target=generate_data)
        gen_thread.daemon = True
        gen_thread.start()
        
        # 启动数据发送线程
        send_thread = threading.Thread(target=send_data)
        send_thread.daemon = True
        send_thread.start()
        
        # 保持主线程运行
        while True:
            time.sleep(1)
    except socketio.exceptions.ConnectionError as e:
        print(f"Connection failed: {e}")
    except KeyboardInterrupt:
        sio.disconnect()
        print("Disconnected")





































# import numpy as np
# import time
# import threading
# import socketio
# import logging

# from lerobot.common.robot_devices.control_configs import ControlPipelineConfig
# from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
# import draccus
# import torch


# cli_args = ['--robot.type=so101', '--robot.cameras={}', '--control.type=teleoperate']

# cfg = draccus.parse(config_class=ControlPipelineConfig, config_path=None, args=cli_args)
# robot = ManipulatorRobot(cfg.robot)


# if not robot.is_connected:
#     robot.connect()


# # 启用详细日志
# logging.basicConfig(level=logging.INFO)

# sio = socketio.Client(logger=True, engineio_logger=True)

# # SERVER_URL = "http://127.0.0.1:5000"
# SERVER_URL = "http://47.109.17.68:8868"

# def generate_and_send_data():
#     """生成随机numpy数据并发送"""
#     while True:
#         if sio.connected:
#             # 生成随机numpy数组 (fp32, 长度6)
#             leader_pos = {}
#             for name in robot.leader_arms:
#                 leader_pos[name] = robot.leader_arms[name].read("Present_Position")
#             data = leader_pos['main']

#             # 转换为列表以便JSON序列化
#             data_list = data.tolist()
            
#             # 通过SocketIO发送数据
#             sio.emit('numpy_data', data_list, namespace='/numpy')
#             print(f"Sent numpy data: {data_list}")
#         else:
#             print("Not connected to server, attempting to reconnect...")
#             try:
#                 sio.connect(SERVER_URL, namespaces=['/numpy'])
#             except Exception as e:
#                 print(f"Reconnect failed: {e}")
#         time.sleep(0.001)  # 每2秒发送一次

# @sio.event(namespace='/numpy')
# def connect():
#     print('Connected to server on /numpy namespace')

# @sio.event(namespace='/numpy')
# def disconnect():
#     print('Disconnected from /numpy namespace')

# if __name__ == '__main__':
#     try:
#         # 连接到服务端
#         sio.connect(SERVER_URL, namespaces=['/numpy'])
#         print(f"Connected to {SERVER_URL} on namespaces: /numpy")
        
#         # 启动数据生成和发送线程
#         data_thread = threading.Thread(target=generate_and_send_data)
#         data_thread.daemon = True
#         data_thread.start()
        
#         # 保持主线程运行
#         while True:
#             time.sleep(1)
#     except socketio.exceptions.ConnectionError as e:
#         print(f"Connection failed: {e}")
#     except KeyboardInterrupt:
#         sio.disconnect()
#         print("Disconnected")